//
// Created by YangShuai on 2022/12/28.
//

/****************************************************************************
 *  Copyright (C) 2018 RoboMaster.
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation, either version 3 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of 
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  You should have received a copy of the GNU General Public License
 *  along with this program. If not, see <http://www.gnu.org/licenses/>.
 ***************************************************************************/
/** @file kalman_filter.c
 *  @version 1.0
 *  @date Apr 2018
 *
 *  @brief kalman filter realization
 *
 *  @copyright 2018 DJI RoboMaster. All rights reserved.
 *
 */

#include "kalman_filter.h"

void kalman_filter_init(kalman_filter_t *F, kalman_filter_init_t *I)
{
	mat_init(&F->xhat,2,1,(float *)I->xhat_data);
	mat_init(&F->xhatminus,2,1,(float *)I->xhatminus_data);
	mat_init(&F->z,2,1,(float *)I->z_data);
	mat_init(&F->A,2,2,(float *)I->A_data);
	mat_init(&F->H,2,2,(float *)I->H_data);
	mat_init(&F->Q,2,2,(float *)I->Q_data);
	mat_init(&F->R,2,2,(float *)I->R_data);
	mat_init(&F->P,2,2,(float *)I->P_data);
	mat_init(&F->Pminus,2,2,(float *)I->Pminus_data);
	mat_init(&F->K,2,2,(float *)I->K_data);
	mat_init(&F->AT,2,2,(float *)I->AT_data);
	mat_trans(&F->A, &F->AT);
	mat_init(&F->HT,2,2,(float *)I->HT_data);
	mat_trans(&F->H, &F->HT);
}



float *kalman_filter_calc(kalman_filter_t *F, float signal1, float signal2)
{
    const float ones_data[4] = {1,0,0,1};
	float TEMP_data[4] = {0, 0, 0, 0};
	float TEMP_data21[2] = {0, 0};
	mat ONES,TEMP,TEMP21;

    mat_init(&ONES,2,2,(float *)ones_data);
	mat_init(&TEMP,2,2,(float *)TEMP_data);
	mat_init(&TEMP21,2,1,(float *)TEMP_data21);

	F->z.pData[0] = signal1;
	F->z.pData[1] = signal2;
    /**
     * 卡尔曼滤波先验估计
     **/
	//1. xhat'(k)= A xhat(k-1)
	mat_mult(&F->A, &F->xhat, &F->xhatminus);

	//2. P'(k) = A P(k-1) AT + Q
	mat_mult(&F->A, &F->P, &F->Pminus);
	mat_mult(&F->Pminus, &F->AT, &TEMP);
	mat_add(&TEMP, &F->Q, &F->Pminus);

    /**
     * 卡尔曼滤波更新数据
     **/
	//3. K(k) = P'(k) HT / (H P'(k) HT + R)
	mat_mult(&F->H, &F->Pminus, &F->K);
	mat_mult(&F->K, &F->HT, &TEMP);
	mat_add(&TEMP, &F->R, &F->K);

	mat_inv(&F->K, &F->P);
	mat_mult(&F->Pminus, &F->HT, &TEMP);
	mat_mult(&TEMP, &F->P, &F->K);

	//4. xhat(k) = xhat'(k) + K(k) (z(k) - H xhat'(k))
	mat_mult(&F->H, &F->xhatminus, &TEMP21);
	mat_sub(&F->z, &TEMP21, &F->xhat);
	mat_mult(&F->K, &F->xhat, &TEMP21);
	mat_add(&F->xhatminus, &TEMP21, &F->xhat);

	//5. P(k) = (1-K(k)H)P'(k)
	mat_mult(&F->K, &F->H, &F->P);
	mat_sub(&ONES, &F->P, &TEMP);
	mat_mult(&TEMP, &F->Pminus, &F->P);

	F->filtered_value[0] = F->xhat.pData[0];
	F->filtered_value[1] = F->xhat.pData[1];
	return F->filtered_value;
	////////x(k+1)[1]=x(k)[1]+0.5*差,x(k+1)[2]=x(k)[2]+9/5010*差
}
